
#include "aquisition_i2c.h"

// global variables
s32 Xa;
s32 Ya;
s32 Za;

s32 Xbias=0;
s32 Ybias=0;
s32 Zbias=0;
s32 Xfs1g=0;
s32 Yfs1g=0;
s32 Zfs1g=0;

void acquisition_i2cInit(void)
{
	// initialize i2c
	i2cInit();
	i2cSetBitrate(100);
	// initialize the timer system
	//timerInit();

	// set pullup resistors
	sbi(PORTD,0);
	sbi(PORTD,1);

	//lis3l02Init();
	// self-calibrate
	//rprintf("Calibrating for level reading...\n");
	calibrate();
	//rprintf("Done!\n");
	//timerPause(5000);
}

void diglevel(s16* tab)
{

	s16 Xtilt;
	s16 Ytilt;
	s16 Ztilt;
	double Xc;
	double Yc;
	double Zc;

	// sample the sensor
	sample();

	// print the raw results to the serial port
	/*rprintf("  X:"); rprintfNum(10,6,TRUE,' ',Xa);
	rprintf("  Y:"); rprintfNum(10,6,TRUE,' ',Ya);
	rprintf("  Z:"); rprintfNum(10,6,TRUE,' ',Za);*/

	// gravity-vector angle calculation

	// apply scale factor
	Xc = (double)Xa/Xfs1g;
	Yc = (double)Ya/Yfs1g;
	Zc = (double)Za/Zfs1g;

	// limit input range for trig functions to follow
	Xc = MIN(Xc, 1);
	Xc = MAX(Xc, -1);
	Yc = MIN(Yc, 1);
	Yc = MAX(Yc, -1);

	// calculate vector angles
//		Xtilt = asin(Xc)*((180*10)/PI);			// asin method
//		Ytilt = asin(Yc)*((180*10)/PI);			// asin method
	Xtilt = atan2(Xc,Zc)*(180/M_PI);	// atan method
	Ytilt = atan2(Yc,Zc)*(180/M_PI);	// atan method

	Ztilt = atan2((double)Ya,(double)Xa)*(180/M_PI);
	if (Ztilt < 0) Ztilt+=360;

	// print results to LCD display
	/*rprintf(" | ");
	rprintf("  Xtilt= "); rprintfNum(10,4,TRUE,' ',Xtilt);
	rprintf("  Ytilt= "); rprintfNum(10,4,TRUE,' ',Ytilt);
	rprintf(" degrees");
	rprintfCRLF();*/

	tab[0]= Xtilt;
	tab[1]= Ytilt;
	tab[2] = Ztilt;

	//timerPause(50);
}


void sample(void)
{
	u08 i;
	Xa = 0;
	Ya = 0;
	Za = 0;

	// average over 50 readings
	for(i=0; i<50; i++)
	{
		Xa += lis3l02GetAccel(0)+Xbias;
		Ya += lis3l02GetAccel(1)+Ybias;
		Za += lis3l02GetAccel(2)+Zbias;
	}

	// normalize the average
	Xa /= 50;
	//rprintf("%d\n",Xa);
	Ya /= 50;
	//rprintf("%d\n",Ya);
	Za /= 50;
	//rprintf("%d\n",Za);

	lis3l02Init();
}

void calibrate(void)
{
	// this calibration routine assumes that the sensor is level and sensing 1g in the Z axis
	// additional orientations are needed for complete calibration

	// sample the sensor
	sample();

	// record bias in X,Y
	Xbias = -Xa;
	Ybias = -Ya;
	// cannot resolve bias in Z using this sensor orientation, so record zero bias
	Zbias = 0;

	// set scale factors for X,Y,Z
//	Xfs1g = lis3l02GetAccel(2);
//	Yfs1g = lis3l02GetAccel(2);
//	Zfs1g = lis3l02GetAccel(2);
//	Xfs1g = 16384;
//	Yfs1g = 16384;
	Xfs1g = 16380;
	Yfs1g = 16380;
	Zfs1g = 16380;
}
